#ifndef GAME_ANIMATION_COMPUTE_HEIGHTMAP_POINT_H
#define GAME_ANIMATION_COMPUTE_HEIGHTMAP_POINT_H
//
// Game Animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <animation/configuration.h>

#include <OpenTissue/core/geometry/geometry_plane.h>
#include <OpenTissue/core/geometry/geometry_barycentric.h>

namespace animation
{

  /**
  * Compute Height Map Point.
  * Given x and y location in world space this method determines the projection
  * along the z-axis onto the height map. It also computes the normal of the
  * height map surface at the projected point.
  *
  * This is usefull for placing or moving things around on the height map surface.
  *
  * @param hmap     The vertices of the heightmap (stored in a geometry_type).
  * @param I        The number of heightmap cells along the x-axis.
  * @param J        The number of heightmap cells along the y-axis.
  * @param width    The size of heightmap along teh x-axis.
  * @param height   The size of heightmap along teh y-axis.
  * @param x        The x-position of the point we want to compute.
  * @param y        The y-position of the point we want to compute.
  * @param p        Upon return this argument holds the computed point.
  * @param p        Upon return this argument holds the corresponding normal of the computed point.
  */
  template<typename geometry_type,typename real_type,typename vector3_type>
    void compute_heightmap_point(
      geometry_type const & hmap
    , unsigned int I
    , unsigned int J
    , real_type width
    , real_type height
    , real_type x
    , real_type y
    , vector3_type & p
    , vector3_type & n
    )
  {
    typedef typename geometry_type::math_types        math_types;
    typedef typename geometry_type::vertex_type       vertex_type;
    typedef OpenTissue::geometry::Plane<math_types>   plane_type;

    real_type dx = width /I;    ///< Spacing of heightmap cells
    real_type dy = height/J;
    real_type offset_x = hmap.get_vertex(0).m_coord[0];
    real_type offset_y = hmap.get_vertex(0).m_coord[1];
    real_type rel_x = x-offset_x;
    real_type rel_y = y-offset_y;
    unsigned int i = static_cast<unsigned int>( std::floor(rel_x/dx) );
    unsigned int j = static_cast<unsigned int>( std::floor(rel_y/dy) );

    vertex_type g0 = hmap.get_vertex(     j*(I+1) +  i    );
    vertex_type g1 = hmap.get_vertex(     j*(I+1) + (i+1) );
    vertex_type g2 = hmap.get_vertex( (j+1)*(I+1) + (i+1) );
    vertex_type g3 = hmap.get_vertex( (j+1)*(I+1) +  i    );

    vector3_type p0 = vector3_type( g0.m_coord[0], g0.m_coord[1], g0.m_coord[2]);
    vector3_type n0 = vector3_type( g0.m_normal[0], g0.m_normal[1], g0.m_normal[2] );
    vector3_type p1(0,0,0);
    vector3_type n1(0,0,0);
    vector3_type p2(0,0,0);
    vector3_type n2(0,0,0);

    //--- we know the cell triangulation pattern of the heightmap looks like
    //---
    //---     p
    //---     |
    //---  g1 |      g2
    //---   +-|----+
    //---   | *   /|
    //---   |    / |
    //---   |   /  |
    //---   |  /   |
    //---   | /    |
    //---   |/     |
    //---   +------+
    //---  g0      g1
    //---
    //--- To pick either triangle g012 or g021 we use a sign-test
    //---
    //---  If
    //---
    //---   (g2-g0) X (p-g0)
    //---
    //---  Is positive then p is above diagonal otherwise p is on or below the diagonal.
    //---
    real_type test = dx*(y-p0(1)) - dy*(x-p0(0));
    if(test>0)
    {
      //--- g0,g2,g3 make up the triangle
      p1 = vector3_type( g2.m_coord[0], g2.m_coord[1], g2.m_coord[2] );
      n1 = vector3_type( g2.m_normal[0], g2.m_normal[1], g2.m_normal[2] );
      p2 = vector3_type( g3.m_coord[0], g3.m_coord[1], g3.m_coord[2] );
      n2 = vector3_type( g3.m_normal[0], g3.m_normal[1], g3.m_normal[2] );
    }
    else
    {
      //--- g0,g1,g2 make up the triangle
      p1 = vector3_type( g1.m_coord[0], g1.m_coord[1], g1.m_coord[2] );
      n1 = vector3_type( g1.m_normal[0], g1.m_normal[1], g1.m_normal[2] );
      p2 = vector3_type( g2.m_coord[0], g2.m_coord[1], g2.m_coord[2] );
      n2 = vector3_type( g2.m_normal[0], g2.m_normal[1], g2.m_normal[2] );
    }
    plane_type plane(p0,p1,p2);
    //--- From plane equation:
    //---
    //---    0 = n*p - w
    //---
    //--- only unknown is p_z, notice that n_z is non-zero otherwise we would
    //--- not be working on a heightmap.
    real_type z = (plane.w() - plane.n()(0)*x  - plane.n()(1)*y)/plane.n()(2);
    p = vector3_type(x,y,z);
    real_type a0=0,a1=0,a2=0;//--- barycentric coords of p!!!
    OpenTissue::geometry::barycentric_algebraic(p0,p1,p2,p,a0,a1,a2);
    n = a0*n0 + a1*n1 + a2*n2;
  }

} // namespace animation

// GAME_ANIMATION_COMPUTE_HEIGHTMAP_POINT_H
#endif
